
#ifndef __clock_c__
#define __clock_c__

#include <zneo.h>

#include "clock.h"

#define CLOCK_UNLOCK_STEP1 0xE7
#define CLOCK_UNLOCK_STEP2 0x18

#define CLOCK_INTERNAL_ON 0x80
#define CLOCK_EXTERNAL_ON 0x40
#define CLOCK_WDTEN 0x20
#define CLOCK_POFEN 0x10
#define CLOCK_WDFEN 0x08
#define CLOCK_FLPEN 0x04
#define CLOCK_USE_INTERNAL 0x00
#define CLOCK_USE_EXTERNAL 0x01
#define CLOCK_USE_WATCHDOG 0x11


// Shamelessly stolen from the ZiLOG Samples
void InitSystemClock(void) {
	unsigned char temp_image;

#if ( SYS_CLK_FREQ == EXTERNAL_CLK_FREQ )	
	unsigned int clock_delay;

	if( (OSCCTL & 0x42) != 0x42 )
	{
		temp_image = OSCCTL;								
		OSCCTL = CLOCK_UNLOCK_STEP1;		
		OSCCTL = CLOCK_UNLOCK_STEP2;

		OSCCTL = CLOCK_INTERNAL_ON | CLOCK_EXTERNAL_ON;

		clock_delay = 2000;
		while (clock_delay--);

		temp_image &= 0x3C; //~(CLOCK_INTERNAL_ON | CLOCK_EXTERNAL_ON | CLOCK_USE_WATCHDOG)
		temp_image |= CLOCK_EXTERNAL_ON | CLOCK_USE_EXTERNAL;

		OSCCTL = CLOCK_UNLOCK_STEP1;		
		OSCCTL = CLOCK_UNLOCK_STEP2;		
		OSCCTL = temp_image;	
	}
#else
	if( (OSCCTL & 0x87) != 0x80 )		
	{
		temp_image = OSCCTL;
		temp_image &= //~(CLOCK_INTERNAL_ON | CLOCK_EXTERNAL_ON | CLOCK_USE_WATCHDOG)
			temp_image |= CLOCK_INTERNAL_ON | CLOCK_USE_INTERNAL;

		OSCCTL = CLOCK_UNLOCK_STEP1;	
		OSCCTL = CLOCK_UNLOCK_STEP2;	
		OSCCTL = temp_image;
	}	
#endif
}

#endif